#include<Matrix/LB_Sparse_Matrix.h>

void lb_sparse_matrix_free(LB_Sparse_Matrix*lsm)
{
    if(lsm==NULL)
    {
        return ;
    }
    for(int i=0;i<lsm->rows;i++)
    {
        LB_Sparse_Matrix_Node*q=lsm->rhead[i];
        while(q!=NULL)
        {
            LB_Sparse_Matrix_Node* temp= q->right;
            lb_sparse_matrix_node_free(q);
            q=temp;
        }
    }
    SAFE_FREE(lsm->rhead);SAFE_FREE(lsm->chead);
    SAFE_FREE(lsm);
}
void lb_sparse_matrix_insert(LB_Sparse_Matrix*lsm,int i,int j,double data)
{
    LB_Sparse_Matrix_Node* p=(LB_Sparse_Matrix_Node*)malloc(sizeof(LB_Sparse_Matrix_Node));
    p->i=i;p->j=j;p->data=data;p->right=NULL;p->down=NULL;
    LB_Sparse_Matrix_Node*q=NULL; 
    if(NULL==lsm->rhead[i]||lsm->rhead[i]->j>j)
    {
        p->right=lsm->rhead[i];
        lsm->rhead[i]=p;
    } 
    else
    {
        for(q=lsm->rhead[i];(q->right)&&q->right->j<j;q=q->right); 
        if(q->right&&q->right->j==j)
        {
            free(p);
            q->right->data=data;
            return ;
        }
        else
        {
            p->right=q->right;
            q->right=p;
        }
    }

    if(NULL==lsm->chead[j]||lsm->chead[j]->i>i)
    {
        p->down=lsm->chead[j];
        lsm->chead[j]=p;
    }
    else
    {
        for(q=lsm->chead[j];(q->down)&&q->down->i<i;q=q->down);
        if(q->down&&q->down->i==i)
        {
            free(p);
            q->down->data=data;
        }
        else
        {
            p->down=q->down;
            q->down=p;
        } 
    }
}
static inline void conjugate_gradient_compute_residual(LB_Sparse_Matrix* A,double* b,double* x,double* re)
{
    double *re1=lb_sparse_matrix_mult_array(A,x);
    for(int i=0;i<A->rows;i++)
    {
        re[i]= b[i]-re1[i];
    }
    free(re1);
}


static inline double conjugate_gradient_compute_alpha(LB_Sparse_Matrix* A,double* r,double* p)
{
    double re=inner_product(r,r,A->rows);

    double* temp=lb_sparse_matrix_mult_array(A,p);
    printf("end mult aary\n");
    re/= inner_product(p, temp,A->rows);
    
    //printf("alpha:%lf\n",re);
    free(temp);
    return re;
}


double* compute_conjugate_gradient(LB_Sparse_Matrix*lsm,double* b)
{
    double* xi=(double*)malloc(sizeof(double)*lsm->rows);
    double* ri=(double*)malloc(sizeof(double)*lsm->rows);
    double* pi=(double*)malloc(sizeof(double)*lsm->rows);
    memset(xi,0,sizeof(double)*lsm->rows);
    memset(ri,0,sizeof(double)*lsm->rows);
    memset(pi,0,sizeof(double)*lsm->rows);
    conjugate_gradient_compute_residual(lsm,b,xi,ri);
    for(int i=0;i<lsm->rows;i++)
    {
        pi[i]=ri[i];
       // printf("pi 0:%lf\n",pi[i]); 
    } 
    double threshold=inner_product(ri,ri,lsm->rows);
    int num=0;
    while(threshold>0.001)
    {
        num++;
        if(num>100)
        {
            num=0;
            printf("once\n");

        }
        printf("begin alpha\n");
        double alpha= conjugate_gradient_compute_alpha(lsm,ri,pi);
        for(int j=0;j<lsm->rows;j++)
        {
            xi[j]+=alpha*pi[j];
        } 
        printf("begin residual\n");
        conjugate_gradient_compute_residual(lsm,b,xi,ri);
        printf("end residula\n");
        double temp=inner_product(ri,ri,lsm->rows);
        double beta=temp/threshold;
        threshold=temp;
        printf("begin pi\n");

        for(int j=0;j<lsm->rows;j++)
        {
            pi[j]=beta*pi[j]+ri[j];
        }
    } 
    SAFE_FREE(ri);SAFE_FREE(pi);
    return xi; 
}
